/*
 * Central.cpp
 *
 *  Created on: 19 NOV 2554
 *      Author: xenogenic
 */

#include "Central.h"

//Origin
#define top 0
#define btm 1

//output pin for L293D
#define motorL_A 22
#define motorL_B 23
#define motorL_P 9
#define motorR_A 24
#define motorR_B 25
#define motorR_P 8

//condition
#define conForward sensorReadL<REF&&sensorReadR<REF
#define conRight sensorReadL<REF&&sensorReadR>REF
#define conLeft sensorReadL>REF&&sensorReadR<REF
#define conAllFront sensorReadL>REF&&sensorReadR>REF
#define conAllBottom sensorReadLB>REF&&sensorReadRB>REF

//setup reference b/w black and white
#define REF 200

//setup analog input slot
#define sensorL 0
#define sensorR 1
#define sensorLB 2
#define sensorRB 3

//direction define
#define fw 0
#define turnLeft 1
#define turnRight 2
#define AFK 3
#define bw 4

//setup time
#define small 1
#define medium 100
#define large 500
#define vLarge 1000

//define position
#define headPosX 0
#define headNegX 1800
#define headNegY 900
#define headPosY 2700

long time = millis();

Central::Central(int pow, int initPos) {
	power = pow;
	curHeadPos = initPos;
	if (initPos == 0) {
		initX = 0;
		initY = 7;
		curX = 0;
		curY = 7;
		Origin = top;
	} else if (initPos == 1800) {
		initX = 7;
		initY = 0;
		curX = 7;
		curY = 0;
		Origin = btm;
	}
	pinMode(motorL_A, OUTPUT);
	pinMode(motorL_B, OUTPUT);
	pinMode(motorL_P, OUTPUT);
	pinMode(motorR_A, OUTPUT);
	pinMode(motorR_B, OUTPUT);
	pinMode(motorR_P, OUTPUT);
	blockLength = 30.0;
}

Central::~Central() {

}

int calPower(int pow) {
		return pow * 255 / 100;
}

double timeDiff(){
	if(millis()-time<=1000)
		return (double(millis()-time))/1000.0;
	return 1.0;
}

double timeDiff(int initTime){
	if(millis()-initTime<=3000)
		return (double(millis()-initTime))/3000.0;
	return 1.0;
}

double timeDec(int initTime){
	if(millis()-initTime<=10000){
		return (10000-double(millis()-initTime))/10000.0;
	}
	return 0;
}

//************************************Movement****************************************************************

void Central::forward(int time, int pow) {
//motorL runs clockwise (A=1,B=0)
	analogWrite(motorL_P, calPower(pow));
	digitalWrite(motorL_A, HIGH);
	digitalWrite(motorL_B, LOW);

//motorR runs clockwise (A=1,B=0)
	analogWrite(motorR_P, calPower(pow));
	digitalWrite(motorR_A, HIGH);
	digitalWrite(motorR_B, LOW);

//time
	delay(time);
	stop(0);

}

void Central::backward(int time, int pow) {
//motorL runs anti-clockwise (A=0,B=1)
	analogWrite(motorL_P, calPower(pow));
	digitalWrite(motorL_A, LOW);
	digitalWrite(motorL_B, HIGH);

//motorR runs anti-clockwise (A=0,B=1)
	analogWrite(motorR_P, calPower(pow));
	digitalWrite(motorR_A, LOW);
	digitalWrite(motorR_B, HIGH);

//time
	delay(time);
	stop(0);
}

void Central::left(int time, int pow) {
//motorL stops (A=0,B=0)
	analogWrite(motorL_P, calPower(pow));
	digitalWrite(motorL_A, LOW);
	digitalWrite(motorL_B, LOW);

//motorR runs clockwise (A=1,B=0)
	analogWrite(motorR_P, calPower(pow));
	digitalWrite(motorR_A, HIGH);
	digitalWrite(motorR_B, LOW);

//time
	delay(time);
	stop(0);
}

void Central::right(int time, int pow) {
//motorL runs clockwise (A=1,B=0)
	analogWrite(motorL_P, calPower(pow));
	digitalWrite(motorL_A, HIGH);
	digitalWrite(motorL_B, LOW);

//motorR stops (A=0,B=0)
	analogWrite(motorR_P, calPower(pow));
	digitalWrite(motorR_A, LOW);
	digitalWrite(motorR_B, LOW);

//time
	delay(time);
	stop(0);
}

void Central::spinLeft(int time, int pow) {
//motorL run anti-clockwise (A=0,B=0)
	analogWrite(motorL_P, calPower(pow));
	digitalWrite(motorL_A, LOW);
	digitalWrite(motorL_B, HIGH);

//motorR runs clockwise (A=1,B=0)
	analogWrite(motorR_P, calPower(pow));
	digitalWrite(motorR_A, HIGH);
	digitalWrite(motorR_B, LOW);

//time
	delay(time);
	stop(0);
}

void Central::spinRight(int time, int pow) {
//motorL runs clockwise (A=1,B=0)
	analogWrite(motorL_P, calPower(pow));
	digitalWrite(motorL_A, HIGH);
	digitalWrite(motorL_B, LOW);

//motorR runs anti-clockwise (A=0,B=1)
	analogWrite(motorR_P, calPower(pow));
	digitalWrite(motorR_A, LOW);
	digitalWrite(motorR_B, HIGH);

//time
	delay(time);
	stop(0);
}

void Central::stop(int time) {
//motorL stops (A=0,B=0)
	analogWrite(motorL_P, 0);
	digitalWrite(motorL_A, LOW);
	digitalWrite(motorL_B, LOW);

//motorR stops (A=o,B=0)
	digitalWrite(motorR_A, LOW);
	digitalWrite(motorR_B, LOW);

//time
	if (time != 0)
		delay(time);
}

void Central::turnLeft90() {
	readSensor();
	int initTime = millis();
	if (sensorReadR < REF) {
		while (!(sensorReadR > REF)) {
			left(1, int(double(power)*timeDiff(initTime)));
			readSensor();
		}
	}
	readSensor();
	while (!(sensorReadR < REF)) {
		left(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}
	readSensor();
	while (!(sensorReadR > REF)) {
		left(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}
	findLine();
//turn off motor
	stop(0);

//add pos
	addHeadPos(-900);

//time delay
	delay(20);
}

void Central::turnRight90() {
	readSensor();
	int initTime = millis();
	if (sensorReadL < REF) {
		while (!(sensorReadL > REF)) {
			right(1, int(double(power)*timeDiff(initTime)));
			readSensor();
		}
	}
	readSensor();
	while (!(sensorReadL < REF)) {
		right(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}
	readSensor();
	while (!(sensorReadL > REF)) {
		right(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}

	findLine();

//turn off motor
	stop(0);

//add pos
	addHeadPos(900);

//time delay
	delay(20);
}

void Central::spin_left90() {
	readSensor();
	int initTime = millis();
	if (sensorReadR < REF) {
		while (!(sensorReadR > REF)) {
			spinLeft(1, int(double(power)*timeDiff(initTime)));
			readSensor();
		}
	}
	readSensor();
	while (!(sensorReadR < REF)) {
		spinLeft(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}
	readSensor();
	while (!(sensorReadR > REF)) {
		spinLeft(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}
	findLine();
//turn off motor
	stop(0);

//add pos
	addHeadPos(-900);

//time delay
	delay(20);
}

void Central::spin_right90() {
	readSensor();
	int initTime = millis();
	if (sensorReadL < REF) {
		while (!(sensorReadL > REF)) {
			spinRight(1, int(double(power)*timeDiff(initTime)));
			readSensor();
		}
	}
	readSensor();
	while (!(sensorReadL < REF)) {
		spinRight(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}
	readSensor();
	while (!(sensorReadL > REF)) {
		spinRight(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}
	findLine();
//turn off motor
	stop(0);

//add pos
	addHeadPos(900);

//time delay
	delay(20);
}

void Central::uturnR() {
	readSensor();
	int initTime = millis();
	if (sensorReadR < REF) {
		while (!(sensorReadR > REF)) {
			spinRight(1, int(double(power)*timeDiff(initTime)));
			readSensor();
		}
	}
	readSensor();
	while (!(sensorReadR < REF)) {
		spinRight(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}

//turn off motor
	stop(0);

//add pos
	addHeadPos(1800);

//time delay
	delay(20);
}

void Central::uturnL() {
	if (sensorReadL < REF) {
		while (!(sensorReadL > REF)) {
			spinLeft(1, int(double(power)*timeDiff(initTime)));
			readSensor();
		}
	}
	readSensor();
	while (!(sensorReadL < REF)) {
		spinLeft(1, int(double(power)*timeDiff(initTime)));
		readSensor();
	}
	
//turn off motor
	stop(0);

//add pos
	addHeadPos(-1800);

//time delay
	delay(20);
}

//******************************************************Trackline*************************************************************

void Central::readSensor() {
	sensorReadL = analogRead(sensorL);
	sensorReadR = analogRead(sensorR);
	sensorReadLB = analogRead(sensorLB);
	sensorReadRB = analogRead(sensorRB);
}

void Central::track() {
	if (conForward) {
		forward(small, int(double(power)*timeDiff()));
	} else if (conRight) {
		right(small, int(double(power)*timeDiff()));
	} else if (conLeft) {
		left(small, int(double(power)*timeDiff()));
	} else if (conAllFront) {
		stop(0);
	}
}

void Central::change_direction(int direct) {
	long initTime=millis();
	if (direct == 0) { //forward
		readSensor();
		while (!((conForward) || (conRight) || (conLeft))) {
			forward(1, int(double(power)*timeDiff(initTime)));
			readSensor();
		}
		stop(0);
	} else if (direct == 1) { //turn left 90
		sensorCond((conAllBottom),AFK);
		turnLeft90(power);
	} else if (direct == 2) { //turn right 90
		sensorCond((conAllBottom),AFK);
		turnRight90(power);
	} else if (direct == 3) { //AFK
		stop(0);
	} else if (direct == 4) { //
		backward(vLarge,power);
	}

//update position
	if (curHeadPos == headPosX)
		curX += 1;
	else if (curHeadPos == headNegX)
		curX -= 1;
	else if (curHeadPos == headPosY)
		curY += 1;
	else if (curHeadPos == headNegY)
		curY -= 1;
}

void Central::sensorAll(int direct) {
	readSensor();
	while (!(conAllFront)) {
		track();
		readSensor();
	}
	change_direction(direct);
}

void Central::sensorCond(bool condition, int direct) {
	readSensor();
	while (!(condition)) {
		track();
		readSensor();
	}
	change_direction(direct);
}

void Central::findLine() {
	readSensor();
	for (int i = 1; i <= 2; i++) {
		while (!(sensorReadL < REF && sensorReadR < REF)) {
			if (sensorReadR > REF) {
				spinRight(1, power);
			}
			if (sensorReadL > REF) {
				spinLeft(1, power);
			}
			readSensor();
		}
		delay(20);
	}
}

//********************************************Position**************************************************

int Central::positiveDegree(int degree) {
	if (degree >= 0) {
		return degree % 3600;
	} else if (degree < 0) {
		return 3600 + (degree % 3600);
	} else {
		return 0;
	}
}

void Central::headPosition(int tarPosition) {
	int angle, countR = 0, countL = 0;

	angle = curHeadPos;

	while (positiveDegree(angle) != tarPosition) {
		angle += 900;
		countR++;
	}

	angle = curHeadPos;

	while (positiveDegree(angle) != tarPosition) {
		angle -= 900;
		countL++;
	}

	if (countR <= countL) {
		angle = 900 * countR;
      //TURN R BY (ANGLE) -> 900=TURNRIGHT90,1800=UTURNR
		//***************************************************NEED TO ADD***************************************************
		int uturnRN = angle % 1800;
		int turnRN = angle % 900 - 2*uturnRN;
		if (uturnRN == 1) {
			uturnR();
		}
		for(int i = turnRN; i = 0; i--;) {
			spin_right90();
		}
	} else if (countL < countR) {
		angle = 900 * countL;
    //TURN l BY (ANGLE) -> 900=TURNLEFT90,1800=UTURNL
		//***************************************************NEED TO ADD***************************************************
		int uturnLN = angle % 1800;
		int turnLN = angle % 900 - 2*uturnLN;
		if (uturnLN == 1) {
			uturnL();
		}
		for(int i = turnLN; i = 0; i--;) {
			spin_left90();
		}
	}
	curHeadPos = tarPosition;
}

void Central::posX(int tarX) {
	int diffX;
	if (curX != tarX) {
		if (curX > tarX) {
			headPosition(headNegX);
		} else if (curX < tarX) {
			headPosition(headPosX);
		}

		diffX = abs(curX - tarX);

		for (int i = 1; i < diffX; i++) {
			sensorAll(fw);
		}
		sensorAll(AFK);
	}
}

void Central::posY(int tarY) {
	int diffY;
	if (curY != tarY) {
		if (curY > tarY) {
			headPosition(headNegY);
		} else if (curY < tarY) {
			headPosition(headPosY);
		}

		diffY = abs(curY - tarY);

		for (int i = 1; i < diffY; i++) {
			sensorAll(fw);
		}
		sensorAll(AFK);
	}
}

void Central::goFW(int head, int node) {

	headPosition(head);

	for (int i = 1; i < node; i++) {
		sensorAll(fw);
	}
	sensorAll(AFK);
}

void Central::gotoPos(int tarX, int tarY) {
	posX(tarX);
	posY(tarY);
}

void Central::gotoOrigin() {
	gotoPos(initX, initY);
}

void Central::grabBox() {
}

void Central::putBox() {

}

void Central::gotoCheckPoint() {
	if (Origin == top)
		gotoPos(0, 1);
	else if (Origin == btm)
		gotoPos(0, 0);
}

void Central::addHeadPos(int addPosition) {
	curHeadPos = positiveDegree(curHeadPos + addPosition);
}

//******************************************************Distance***********************************************

double Central::getDistance(int pin) {
	int sumDist = 0;
	for (int i = 1; i <= 3; i++) {
		sumDist += microsecToCen(scan(pin));
	}
	return sumDist / 3;
}

int Central::getNodeDistance(int pin) {
	return int(getDistance(pin) / blockLength + 0.5);
}

double Central::scan(int pin) {

// make clean high pulse
	pinMode(pin, OUTPUT);
	digitalWrite(pin, LOW);
	delayMicroseconds(2);
	digitalWrite(pin, HIGH);
	delayMicroseconds(5);
	digitalWrite(pin, LOW);

// receive signal
	pinMode(pin, INPUT);
	return pulseIn(pin, HIGH);
}

bool Central::checkBox(int pin) {
	double dist = getDistance(pin);

	double travelDist;
	if (curHeadPos == headPosX)
		travelDist = curX;
	else if (curHeadPos == headNegX)
		travelDist = 7 - curX;
	else if (curHeadPos == headPosY)
		travelDist = curY;
	else if (curHeadPos == headNegY)
		travelDist = 7 - curY;

	if (dist > 0 && dist < 200 - travelDist * 30)
		return true;
	else
		return false;
}

double Central::microsecToCen(double dur) {
// 29	 microsec per centimeter.
	return dur / 29 / 2;
}
